#pragma once
#include "hnurm_uart/Serialcodec.h"

#include <rclcpp/rclcpp.hpp>

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>

#include <tf2_ros/transform_broadcaster.h>

#include <nav_msgs/msg/odometry.hpp>

namespace hnurm
{
class UartNode : public rclcpp::Node
{
public:
    using UartNodePtr = std::shared_ptr<UartNode>;

    UartNode(const rclcpp::NodeOptions& options) : rclcpp::Node("hnurm_uart_node", options), logger(get_logger()), serial_codec_(nullptr)
    {
    }

    ~UartNode()
    {
        delete serial_codec_;
    }

    UartNode(const UartNode &)            = delete;
    UartNode &operator=(const UartNode &) = delete;
    UartNode(UartNode &&)                 = delete;
    UartNode &operator=(UartNode &&)      = delete;
    void      run();

private:
    void sub_callback(hnurm_interfaces::msg::VisionSendData::SharedPtr msg);
    void sub_twist_callback(geometry_msgs::msg::Twist::SharedPtr msg);
    void timer_callback();
    void re_launch();

private:
    rclcpp::CallbackGroup::SharedPtr callback_group1_;
    rclcpp::CallbackGroup::SharedPtr callback_group2_;

    rclcpp::Subscription<hnurm_interfaces::msg::VisionSendData>::SharedPtr sub_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr             sub_twist_;
    rclcpp::Publisher<hnurm_interfaces::msg::VisionRecvData>::SharedPtr    pub_;
    std::thread                                                            uart_thread_;

    // tf broadcaster
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Logger               logger;
    SerialCodec                 *serial_codec_;

    // topics
    std::string recv_topic_;
    std::string send_topic_;
    float       control_id_ = 0;

    bool stop_flag_ = false;
    int  error_cnt_ = 0;

protected:
    UartNodePtr shared_from_this()
    {
        return std::static_pointer_cast<UartNode>(rclcpp::Node::shared_from_this());
    }
};
}
